#include <cmath>

#include "Stick.h"

namespace BlobSalladDS
{
	Stick::Stick(PointMass *pointMassA, PointMass *pointMassB)
	{
		this->length = pointMassDist(*pointMassA, *pointMassB);
		this->lengthSquared = this->length * this->length;
		this->pointMassA = pointMassA;
		this->pointMassB = pointMassB;
		this->delta = new Vector(0.0, 0.0);
	}

	double Stick::pointMassDist(PointMass &pointMassA, PointMass &pointMassB)
	{
		double aXbX, aYbY;

		aXbX = pointMassA.getXPos() - pointMassB.getXPos();
		aYbY = pointMassA.getYPos() - pointMassB.getYPos();

		return sqrt(aXbX * aXbX + aYbY * aYbY);
	}

	PointMass *Stick::getPointMassA()
	{
		return this->pointMassA;
	}

	PointMass *Stick::getPointMassB()
	{
		return this->pointMassB;
	}

	void Stick::scale(double scaleFactor)
	{
		this->length *= scaleFactor;
		this->lengthSquared = this->length * this->length;
	}

	void Stick::sc(Environment &env)
	{
		double dotProd, scaleFactor;
		Vector *pointMassAPos, *pointMassBPos;

		pointMassAPos = this->pointMassA->getPos();
		pointMassBPos = this->pointMassB->getPos();

		this->delta->set(*pointMassBPos);
		this->delta->sub(*pointMassAPos);

		dotProd = this->delta->dotProd(*(this->delta));

		scaleFactor = this->lengthSquared / (dotProd + this->lengthSquared) - 0.5;
		this->delta->scale(scaleFactor);

		pointMassAPos->sub(*(this->delta));
		pointMassBPos->add(*(this->delta));
	}

	void Stick::setForce(Vector &force)
	{
		this->pointMassA->setForce(force);
		this->pointMassB->setForce(force);
	}

	void Stick::addForce(Vector &force)
	{
		this->pointMassA->addForce(force);
		this->pointMassB->addForce(force);
	}

	void Stick::move(double dt)
	{
		this->pointMassA->move(dt);
		this->pointMassB->move(dt);
	}

	void Stick::draw(void * ctx, double scaleFactor)
	{
		//TODO: draw using PAlib
	}
}
